% Main script to run CADAC

% Initialise the constants
CON = CON();

C = zeros(1,3510);


%**********************************************************************
%************************ MODULES FOR ROCKET3 **************************
%**********************************************************************
%*** *
%*** * Calling sequence of Modules:
%*** *   G2   ENVIRONMENT
%*** *   A2   PROPULSION
%*** *   A1   AERODYNAMICS
%*** *   A3   FORCES
%*** *   C2   AUTOPILOT
%*** *   C4   ACTUATOR
%*** *   D1   NEWTONS LAW
%*** * with dummy RETURNs for unused modules


% Initialise variables - Function CADX3.INI1_VARS
C(CON.CRAD) = 57.29577951;

%---  Conversion factor,  CFTM: multiply times feet to obtain meters
C(CON.CFTM) = .3048006;

%---  Conversion factor, CKFPS: multiply times knots to obtain ft/sec.
C(CON.CKFPS) = 1.6878;

%---  Radius of the Earth (ft)
C(CON.REARTH) = 20902190.;

%--- Gravitational parameter mu for the Earth (ft^3/sec^2)
C(CON.AMU) = 1.407645E+16;

%--- Acceleration due to gravity (ft/sec^2)
C(CON.AGRAV)  = 32.174;


%---  WEII3 = omega = angular rotation of the earth (rad/sec)
%     This variable is initialized to the rotating earth model 
%     (OPNORO=0).  If a NON rotating earth is desired, the OPNORO flag 
%     should be set to 1.0 with a type 3 card.
C(CON.WEII3) = 7.2921154E-05;




%*** ******************************************************************
%*** * Initializes the equations of motions of Module D1
%*** * Reserved C(3510) locations are 1600-1699
%*** * This module performs the following functions
%*** *
%*** * 1) Define the locations of the state and state derivative
%*** *    variables
%*** * 2) Converts geographic inputs into inertial coordinates
%*** *
%*** * MODIFICATION HISTORY
%*** * 960711 Created by Peter Zipfel
%*** *
%*** ******************************************************************

%persistent dum3 ipl iplv sbie sbii teg tge tgv tie tig tvg vbeg vbei vbii weii ; 
dvbe=[];
psivg=[];
thtvg=[];
blon=[];
blat=[];


ipl=zeros(1,100);
iplv=zeros(1,100);
vbeg=zeros(1,3);
tge=zeros(3,3);
teg=zeros(3,3);
sbie=zeros(1,3);
tvg=zeros(3,3);
tgv=zeros(3,3);
tie=zeros(3,3);
sbii=zeros(1,3);
tig=zeros(3,3);
weii=zeros(3,3);
dum3=zeros(1,3);
vbei=zeros(1,3);
vbii=zeros(1,3);

%*** INPUT DATA INITIALIZATION
psivgx = C(CON.PSIVGX);
thtvgx = C(CON.THTVGX);
blon = C(CON.BLON);
blat = C(CON.BLAT);
balt = C(CON.BALT);
baltft = C(CON.BALTFT);
dvbe = C(CON.DVBE);

% PSIVGX = I Heading angle from north - deg
% THTVGX = I Flight path angle from horizontal - deg
% BLON = I/G Vehicle longitude - rad
% BLAT = I/G Vehicle latitude - rad
% BALT = I/O Vehicle altitude - m
% BALTFT = I/O Vehicle altitude - ft
% DVBE = I/G Geographic speed - m/s

%*** INPUT FROM EXECUTIVE

% equivalence(bcom_1(0058),weii3) ::;
% equivalence(bcom_1(2562),ipl(1)) ::;
% equivalence(bcom_1(2867),iplv(1)) ::;

rearth = C(CON.REARTH);
crad = C(CON.CRAD);
nip = C(CON.NIP);

% IPL(100) = E State derivitave bcom_1-array locations
% IPLV(100) = E State bcom_1-array locations
% N = E Number of variables to integrate

%*** INITIALIZATION

% equivalence(bcom_1(1622),tgv(1,1)) ::;
% equivalence(bcom_1(1631),tig(1,1)) ::;
% equivalence(bcom_1(1649),sbii(1)) ::;
% equivalence(bcom_1(1643),vbii(1)) ::;
% equivalence(bcom_1(1658),balt0) ::;

%***  INITIALIZATION OF STATE VARIABLES

%iloc=1640;
%for i=0:2;
%ipl(nip)=fix(iloc+i);
%iplv(nip)=fix(iloc+i+3);
%nip=nip+1;
%end; i=2+1;

%iloc=1646;
%for i=0:2;
%ipl(nip)=fix(iloc+i);
%iplv(nip)=fix(iloc+i+3);
%nip=nip+1;
%end; i=2+1;

%***INPUT CONVERSION TO SBII AND VBII AND INITIAL TGV AND TIG

sbie(1)=(balt+rearth).*cos(blat).*cos(blon);
sbie(2)=(balt+rearth).*cos(blat).*sin(blon);
sbie(3)=(balt+rearth).*sin(blat);
%[tie]=matuni(tie,3);
[tie] = zeros(3);
%sbii*tie,sbie,3,3,1);

psivg=psivgx./crad;
thtvg=thtvgx./crad;
[vbeg,dvbe,psivg,thtvg]=matcar(vbeg,dvbe,psivg,thtvg);
[tge,blon,blat]=cadtge3(tge,blon,blat);
%[teg,tge]=mattra(teg,tge,3,3);
teg = transpose(tge);
%[weii]=matzer(weii,3,3);
[weii] = zeros(3,3);

% Putting this in to get to work??
weii3 = 0;

weii(1,2)=-weii3;
weii(2,1)=weii3;
%dum3*weii,sbii,3,3,1);
%tig*tie,teg,3,3,3);
%vbei*tig,vbeg,3,3,1);
%[vbii,vbei,dum3]=matadd(vbii,vbei,dum3,3,1);

% Correct the original code
vbii = vbei + dum3;

[tvg,psivg,thtvg]=mat2tr(tvg,psivg,thtvg);

%[tgv,tvg]=mattra(tgv,tvg,3,3);

tgv = transpose(tvg);

%*** SAVE LAUNCH ALTITUDE

balt0=balt;
baltft=balt.*3.2808399;
% End D1I



% Test call each function

C = A1(C,CON);
C = A2I(C,CON);
C = A2(C,CON);
C = A3(C,CON);

blon = 100.0;
blat = 20.0;
balt = 1000;
dbi = 100;
sbie = [1000;1000;1000];

C(CON.REARTH) = 1000;


[blon,blat,balt,dbi,sbie]=cadsph3(blon,blat,balt,dbi,sbie,C,CON);